#include "LPC11xx.h"
#include "utilfun.h"
#include "gpio.h"
#include "timer16.h"
#include "timer32.h"
//#include "uart.h"
#include "utilio.h"
#include "math.h"

/***************************************************************
 * Macro definitions                                           *
 ***************************************************************/
#define us2TimerInterval(us) ( us * (SystemAHBFrequency / 1000000) )
#define ms2TimerInterval(ms) ( ms * (SystemAHBFrequency / 1000) )

#define SONIC_SPEED     346
#define ECHO_COUNT_MAX  5

/*
 * There are two level of power control.
 */
#define ALLOW_LOW_CROSSTALK_DELAY 0

#define LOW_POWER_LEVEL     0
#define LOW_PWM_DELAY       1           // us
#define LOW_CROSSTALK_DELAY 400
#define LOW_ECHO_WAITE      30          // ms, > 2.5m*2
#define LOW_JITTER_DELAY    (LOW_PWM_DELAY/2)

#define HIGH_POWER_LEVEL        0
#define HIGH_PWM_DELAY          1000
#define HIGH_CROSSTALK_DELAY    1000
#define HIGH_JITTER_DELAY       (HIGH_PWM_DELAY/2)


#define HTRD             23              // mm, TX and RX distance.
#define LOFFSET          3.5f            // mm, software distance - OFFSET = physical distance.
#define DOFFSET          73              // mm, last distance.

/***************************************************************
 * Globol variables                                            *
 ***************************************************************/
static volatile uint32_t echoAverageTime;
static volatile uint32_t echoReceivedCount;
static volatile uint32_t echoReceivedTime;

static volatile uint32_t power_level;

/***************************************************************
 * Functions                                                   *
 ***************************************************************/
//static void myDelay (uint32_t ulTime)
//{
//    uint32_t i;
//
//    i = 0;
//    while (ulTime--) {
//        for (i = 0; i < 5000; i++);
//    }
//}

static void startRanging(void)
{
// first, use a short delay PWM to read short distance exactely.
    power_level = LOW_POWER_LEVEL;
    
    Timer32Reset(0);
    Timer32Enable(0, ms2TimerInterval(500));    // long time enough.
    
    // sending sonic.
    Timer16Enable(0);
    Timer32DelayUs(1, LOW_PWM_DELAY);
    Timer16Disable(0);
    Timer16Reset(0);
    
#if (ALLOW_LOW_CROSSTALK_DELAY)
    // waiting a time to avoid crosstalk.
    Timer32DelayUs(1, LOW_CROSSTALK_DELAY);
#endif
    
    GPIOIntEnable(0, 3);    // start listening echo.
    echoAverageTime = 0;    // clear now to avoid software bugs(interrupt immediate).
    echoReceivedTime = 0;
    echoReceivedCount = 0;
    
    // waiting for short distance echo.
    Timer32DelayMs(1, LOW_ECHO_WAITE);
    
// if get the distance, return now.
    if (echoAverageTime > 0) {
        return;
    }
    
// else, go to high power level.
    GPIOIntDisable(0, 3);
    
    power_level = HIGH_POWER_LEVEL;
    
    Timer32Reset(0);
    Timer32Enable(0, ms2TimerInterval(500));    // waiting long time enough.
    
    // sending sonic.
    Timer16Enable(0);
    Timer32DelayUs(1, HIGH_PWM_DELAY);
    Timer16Disable(0);
    Timer16Reset(0);
    
    // waiting a time to avoid crosstalk.
    Timer32DelayUs(1, HIGH_CROSSTALK_DELAY);
    
    GPIOIntEnable(0, 3);    // start listening echo.
    echoAverageTime = 0;
    echoReceivedTime = 0;
    echoReceivedCount = 0;
    
    // return to main to waiting long time.
}

static void sonicCome(void* mis)
{
    uint32_t intPins = *(uint32_t*)mis;
    
    // save time as soon as sonic come.
    echoAverageTime = LPC_TMR32B0->TC;
    
    // is PIOn_3
    if ( (intPins >> 3) & 0x01 ) {
        // if down-edge not real (have jitter).
        Timer16DelayUs(1, (power_level == LOW_POWER_LEVEL)
            ? LOW_JITTER_DELAY : HIGH_JITTER_DELAY);
        if (GPIOGetValue(0, 3) == 1) {
            // no echo.
            echoAverageTime = 0;
            return;
        }

        ++echoReceivedCount;
        
        if (echoReceivedCount == 1) {
            GPIOIntDisable(0, 3);   // stop listening echo.
            Timer32Disable(0);      // stop counting.
            
            return;
        }
        else {
            // no echo.
            echoAverageTime = 0;
        }
    }
    else {
        // no echo.
        echoAverageTime = 0;
    }
    
    // no echo.
    echoAverageTime = 0;
}
//
//static void waitingTimeout(void* unuse)
//{
//    GPIOIntDisable(0, 3);   // stop listening echo.
//    Timer32Disable(0);      // stop counting.
//    
//    echoAverageTime = 0;  // no distance.
//}

// return distance in mm.
double calcDistance()
{
    double L = (( SONIC_SPEED * 
                    echoAverageTime/(SystemAHBFrequency/1000000) )
                / 1000 / 2.0) - LOFFSET;     // mm.
    return sqrt(L*L - (double)HTRD*HTRD) - DOFFSET;
}

// this is correct!
//// return distance in mm.
//double calcDistance()
//{
//    return (( SONIC_SPEED * 
//                    echoAverageTime/(SystemAHBFrequency/1000000) )
//                / 1000 / 2) - LOFFSET;
//}

/***************************************************************
 * MAIN                                                        *
 ***************************************************************/
int main (void)
{
    uint32_t period = us2TimerInterval(25);
    SystemInit();             /* MUST BE INCLUDED! */

    GPIOInit();
    GPIOSetDir(PORT2, 7, GPIO_DIR_OUTPUT);
    
    /* Set port 0.3 to down-edge interrupt. */
    GPIOSetDir(PORT0, 3, GPIO_DIR_INPUT);
    GPIOSetInterrupt(PORT0, 3, 0, 0, 0);
    GPIOSetIntCallback(0, sonicCome);
//
//	utilInit();
    
//    UARTInit(9600);
    
    Timer16InitPWM(0, period, MATCH0, 0);
    Timer16SetMatchPWM(0, 0, period/2);
    Timer16Disable(0);
    
    // timer32 1 is for delay.
    Timer32Init(1);
    
    // timer32 0 is for waiting.
    Timer32Init(0);
    Timer32Disable(0);
    //Timer32SetIntCallback(0, waitingTimeout);

	MyStdout=LCD;
	StiInit();
    

	printf("Hello, Sonic!\r\n");
//    UARTSend("Hello, Sonic!\r\n", 15);

    while (1) {
    
        startRanging();
        Timer32DelayMs(1, 500);
        
        if (echoAverageTime) {
            static char running = 0;
            running = !running;
//            uint32_t time = SONIC_SPEED*LPC_TMR32B0->TC / (SystemAHBFrequency/1000000);
//            UARTSend((uint8_t*)&time, 4);
	        printf("\f    Sonic! \r\n");
            printf("---------------%c\r\n", running ? 'R' : '-');
            printf("Distance is:\r\n%03.1f cm\r\n", (double)calcDistance()/10.0f);
        }
        GPIOSetValue(PORT2, 7, !GPIOGetValue(2, 7));
    }
}

/***************************************************************
 * END OF FILE                                                 *
 ***************************************************************/
